%data = load('test1.txt');
%data = load('ranging_data.txt');
%data = load('result.txt');
%data = data';
%pos = data(2:4,:);
%Angle data
%a = 0:360;
%a = (a/2)*pi/180;

rdata = load('ranging_data.txt');
odata = load('odometry_data.txt');
pos = odata(:,2:4);
close all;
plot(pos(:,2),pos(:,3),'rx');
sleep(1);

todo = odata(:,1);
tr = rdata(:,1);
et = todo-tr;


vr = load('virtualscan.txt');

hold on;
axis "equal";
grid on;
%axis([-4 2 -1 3]);


for i=1:10:size(rdata,1);

% Range measurement
a = rdata(i,:);
a = a(2:182);
r = rdata(i,:);
r = r(183:end);
%r = r(5:end);
p = pos(i,:);

[v,ind]=find(r>80);
r(ind)=0;

%%% FIXME:: IN FUTURE NOT LIKE THIS
tmp=p;
p(1) = tmp(2);
p(2) = tmp(3);
p(3) = tmp(1);
%p = [0 0 0];

x = r.*cos(a + p(3)) + p(1);
y = r.*sin(a + p(3)) + p(2);

vx = vr.*cos(a + p(3)) + p(1);
vy = vr.*sin(a + p(3)) + p(2);
 
hold off;
sleep(5)
%axis([-1 5 -5 5]);
plot(x(1:2:end),y(1:2:end),'.x');
hold on;
plot(vx(1:2:end),vy(1:2:end),'gx');
hold on;
plot(p(1),p(2),'gx');


end;
